// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_

#include <svo/global.h>

namespace vk {
    class AbstractCamera;
    namespace patch_score {
        template<int HALF_PATCH_SIZE> class ZMSSD;
    }
}

namespace svo {

    class Point;
    class Frame;
    class Feature;

    /// Warp a patch from the reference view to the current view.
    namespace warp {

        void getWarpMatrixAffine(
                const vk::AbstractCamera& cam_ref,
                const vk::AbstractCamera& cam_cur,
                const Vector2d& px_ref,
                const Vector3d& f_ref,
                const double depth_ref,
                const SE3& T_cur_ref,
                const int level_ref,
                Matrix2d& A_cur_ref);

        int getBestSearchLevel(
                const Matrix2d& A_cur_ref,
                const int max_level);

        void warpAffine(
                const Matrix2d& A_cur_ref,
                const cv::Mat& img_ref,
                const Vector2d& px_ref,
                const int level_ref,
                const int level_cur,
                const int halfpatch_size,
                uint8_t* patch);

//        void warpAlongEpi(
//                const Matrix2d& A_cur_ref,
//                const cv::Mat& img_ref,
//                const Vector2d& px_ref,
//                const int level_ref,
//                const int level_cur,
//                const int length,
//                const Vector2d& dir,
//                uint8_t* patch);

    } // namespace warp

    /// Patch-matcher for reprojection-matching and epipolar search in triangulation.
    class Matcher
    {
        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

                enum MatchResult
                {
                    EdgeDirectionViolate=-3,
                    SubpixAlignFail=-2,
                    SearchRangeLimited=-1,
                    Unexpected=0,
                    Success=1
                };

            static const int halfpatch_size_ = 4;
            static const int patch_size_ = 8;

            typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;

            struct Options
            {
                bool align_1d;              //!< in epipolar search: align patch 1D along epipolar line
                int align_max_iter;         //!< number of iterations for aligning the feature patches in gauss newton
                double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align
                size_t max_epi_search_steps;//!< max number of evaluations along epipolar line
                bool subpix_refinement;     //!< do gauss newton feature patch alignment after epipolar search
                bool epi_search_edgelet_filtering;
                double epi_search_edgelet_max_angle;
                Options() :
                    align_1d(false),
                    align_max_iter(10),
                    max_epi_length_optim(2.0),
                    max_epi_search_steps(1000),
                    subpix_refinement(true),
                    epi_search_edgelet_filtering(true),
                    epi_search_edgelet_max_angle(0.9)
                {}
            } options_;

            uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
            uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
            Matrix2d A_cur_ref_;          //!< affine warp matrix
            Vector2d epi_dir_;
            double epi_length_;           //!< length of epipolar line segment in pixels (only used for epipolar search)
            double h_inv_;                //!< hessian of 1d image alignment along epipolar line
            int search_level_;
            bool reject_;
            Feature* ref_ftr_;
            Vector2d px_cur_;

            Matcher() = default;
            ~Matcher() = default;

            /// Find a match by directly applying subpix refinement.
            // YS: apply an affine warping, determine the best level and search in that level
            /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
            bool findMatchDirect(
                    const Point& pt,
                    const Frame& frame,
                    Vector2d& px_cur);

            /// Find a match by searching along the epipolar line without using any features.
            MatchResult findEpipolarMatchDirect(
                    const Frame& ref_frame,
                    const Frame& cur_frame,
                    const Feature& ref_ftr,
                    const double d_estimate,
                    const double d_min,
                    const double d_max,
                    Vector2d& px_found,
                    double& depth);

            void createPatchFromPatchWithBorder();
    };

} // namespace svo

#endif // SVO_MATCHER_H_
